%%  Simulation

clear all;close all;clc;

for j=1:6
    xDOF(:,j)=[0.5 -0.5];
    zDOF(:,j)=[0 0];
    nDOF(j) =norm(xDOF(:,j)); 
end

n=2;m=1;l=1;dM=4;dm=2;
R2=1*eye(m);R1=1*eye(l);
Alpha=1;Beta=1;
JDOF=0;

u1max=1.97;
y1max=4.5;      

Cu=1;Cy=1;
eps=0.08;r=1;
m1=1+eps;m1=sqrt(m1);
m2=(1+(eps^-1))*(4*r+2);m2=sqrt(m2);
zita1=0.8^2;zita2=0.8^2;eps1=0.3;eps2=0.3;eps3=0.2;                        %New Variables 

% SECOND SYSTEM     Example 1 of MY THIRD PAPER  
el=0.08;
%first sub-system
A10=[1.4 0.2;-0.2 0.2];Ad10=[0 0.01;0.02 0];
B10=[-0.7;1];
E11=0.1*eye(n);F11=0.1*eye(n);G11=[0.1;0];
E11bar=el*E11;F11bar=el*F11;G11bar=el*G11;
F1=[0.01 0;0 0.01];H1=[0.002 0;0 0.002];
%second sub-system
A20=[1.1 0.3;0.5 0.4];Ad20=[0 0.01;0.02 0];
B20=[0.8;-0.5];
C2=[3 0.7];C1=C2;
E12=0.2*eye(n);F12=0.2*eye(n);G12=[0.2;0];
E12bar=el*E12;F12bar=el*F12;G12bar=el*G12;
F2=[0.01 0;0 0.01];H2=[0.002 0;0 0.002];

[U1,C10,V1] = svd(C1);
[U2,C20,V2] = svd(C2);

Vp1=V1(1:n,1:l);Vpp1=V1(1:n,(l+1):(n-l+1));
Vp2=V2(1:n,1:l);Vpp2=V2(1:n,(l+1):(n-l+1));

C10=C10(1:l,1:l);
C20=C20(1:l,1:l);

T0=dM+2;Tf=T0+40;

%***dm=2***
d=[2*ones((Tf-T0)/4,1);3*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1);2*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1)];

%Sx1=sdpvar(n,n);Sx2=sdpvar(n,n);
X11=sdpvar(l,l);X12=sdpvar(l,l);
X21=sdpvar(n-l,n-l);X22=sdpvar(n-l,n-l);
Sz1=sdpvar(n,n);Sz2=sdpvar(n,n);
W1=sdpvar(n,n);W2=sdpvar(n,n);
Y1=sdpvar(m,l,'full');Y2=sdpvar(m,l,'full');
Landa1=sdpvar(1,1);Landa2=sdpvar(1,1);
gama=sdpvar(1,1);
Z11=sdpvar(n,l,'full');Z12=sdpvar(n,l,'full');
Z21=sdpvar(m,n,'full');Z22=sdpvar(m,n,'full');
Z31=sdpvar(n,n,'full');Z32=sdpvar(n,n,'full');
G1=sdpvar(m,m);
G2=sdpvar(l,l);

%C=[Sx1>=0,Sx2>=0,Sz1>=0,Sz2>=0,W1>=0,W2>=0];
C=[X11>=0,X12>=0,X21>=0,X22>=0,Sz1>=0,Sz2>=0,W1>=0,W2>=0];
C=C+[Landa1>=0,Landa2>=0];

% OFFLINE LMI for dm=2

%LMI#1      j=1;s=1;
C=C+[[-Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,2*n) zeros(n,(dM-1)*n) m1*(A10*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')+B10*Y1*C1)' (Y1*C1)'*R2 (Z11*C1)' Vp1*X11*Vp1'+Vpp1*X21*Vpp1' Vp1*X11*Vp1'+Vpp1*X21*Vpp1' (C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))'*R1 (m2*E11bar*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))' (m2*G11bar*Y1*C1)' (m2*Alpha*F1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))' zeros(n,3*n) zeros(n,(dM-dm)*n);...
zeros(n) -W1 zeros(n) zeros(n,(dM-1)*n) m1*(Ad10*W1)' zeros(n,m) zeros(n,3*n)  zeros(n,l) zeros(n,3*n) (m2*F11bar*W1)' (m2*Beta*H1*W1)' zeros(n) zeros(n,(dM-dm)*n);...
zeros(n,2*n) -Sz1 zeros(n,(dM-1)*n) m1*(B10*Z21)' Z21'*R2  Z31' zeros(n,2*n) zeros(n,l) zeros(n,5*n) (m2*G11bar*Z21)' zeros(n,(dM-dm)*n);...
zeros(n*(dM-dm),3*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n*(dm-1)) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),3*n) zeros(n*(dM-dm),l) zeros(n*(dM-dm),6*n) blkdiag(W2,W2);...
zeros(n*(dm-1),3*n) zeros(n*(dm-1),n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),3*n) zeros(n*(dm-1),l) zeros(n*(dm-1),6*n) zeros(n*(dm-1),(dM-dm)*n);...
m1*(A10*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')+B10*Y1*C1) m1*(Ad10*W1) m1*(B10*Z21) zeros(n,(dM-1)*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
R2*(Y1*C1) zeros(m,n) R2*Z21 zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,3*n) zeros(m,l) zeros(m,6*n) zeros(m,(dM-dm)*n);...
Z11*C1 zeros(n) Z31 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -Sz1 zeros(n,2*n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
Vp1*X11*Vp1'+Vpp1*X21*Vpp1' zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W1 zeros(n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
Vp1*X11*Vp1'+Vpp1*X21*Vpp1' zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) -((dM-1)^(-1))*W2 zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
R1*(C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')) zeros(l,2*n) zeros(l,(dM-1)*n) zeros(l,n) zeros(l,m) zeros(l,3*n) -gama*eye(l) zeros(l,6*n) zeros(l,(dM-dm)*n);...
m2*E11bar*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,5*n) zeros(n,(dM-dm)*n);...
m2*G11bar*Y1*C1 zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,4*n) zeros(n,(dM-dm)*n);...
m2*Alpha*F1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,2*n) -Landa1*eye(n) zeros(n,3*n) zeros(n,(dM-dm)*n);...
zeros(n) m2*F11bar*W1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,3*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,2*n) zeros(n,(dM-dm)*n);...
zeros(n) m2*Beta*H1*W1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,4*n) -Landa1*eye(n) zeros(n) zeros(n,(dM-dm)*n);...
zeros(n,2*n) m2*G11bar*Z21 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,5*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,(dM-dm)*n);...
zeros((dM-dm)*n,3*n) blkdiag(W2,W2) zeros((dM-dm)*n,(dm-1)*n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,3*n) zeros((dM-dm)*n,l) zeros((dM-dm)*n,6*n) blkdiag(-W1,-W1)]<=0];

%LMI#1      j=1;s=2;
C=C+[[-Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,2*n) zeros(n,(dM-1)*n) m1*(A10*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')+B10*Y1*C1)' (Y1*C1)'*R2 (Z11*C1)' Vp1*X11*Vp1'+Vpp1*X21*Vpp1' Vp1*X11*Vp1'+Vpp1*X21*Vpp1' (C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))'*R1 (m2*E11bar*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))' (m2*G11bar*Y1*C1)' (m2*Alpha*F1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))' zeros(n,3*n) zeros(n,(dM-dm)*n);...
zeros(n) -W1 zeros(n) zeros(n,(dM-1)*n) m1*(Ad10*W1)' zeros(n,m) zeros(n,3*n)  zeros(n,l) zeros(n,3*n) (m2*F11bar*W1)' (m2*Beta*H1*W1)' zeros(n) zeros(n,(dM-dm)*n);...
zeros(n,2*n) -Sz1 zeros(n,(dM-1)*n) m1*(B10*Z21)' Z21'*R2  Z31' zeros(n,2*n) zeros(n,l) zeros(n,5*n) (m2*G11bar*Z21)' zeros(n,(dM-dm)*n);...
zeros(n*(dM-dm),3*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n*(dm-1)) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),3*n) zeros(n*(dM-dm),l) zeros(n*(dM-dm),6*n) blkdiag(W2,W2);...
zeros(n*(dm-1),3*n) zeros(n*(dm-1),n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),3*n) zeros(n*(dm-1),l) zeros(n*(dm-1),6*n) zeros(n*(dm-1),(dM-dm)*n);...
m1*(A10*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')+B10*Y1*C1) m1*(Ad10*W1) m1*(B10*Z21) zeros(n,(dM-1)*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
R2*(Y1*C1) zeros(m,n) R2*Z21 zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,3*n) zeros(m,l) zeros(m,6*n) zeros(m,(dM-dm)*n);...
Z11*C1 zeros(n) Z31 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -Sz2 zeros(n,2*n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
Vp1*X11*Vp1'+Vpp1*X21*Vpp1' zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W1 zeros(n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
Vp1*X11*Vp1'+Vpp1*X21*Vpp1' zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) -((dM-1)^(-1))*W2 zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
R1*(C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')) zeros(l,2*n) zeros(l,(dM-1)*n) zeros(l,n) zeros(l,m) zeros(l,3*n) -gama*eye(l) zeros(l,6*n) zeros(l,(dM-dm)*n);...
m2*E11bar*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,5*n) zeros(n,(dM-dm)*n);...
m2*G11bar*Y1*C1 zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,4*n) zeros(n,(dM-dm)*n);...
m2*Alpha*F1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,2*n) -Landa2*eye(n) zeros(n,3*n) zeros(n,(dM-dm)*n);...
zeros(n) m2*F11bar*W1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,3*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,2*n) zeros(n,(dM-dm)*n);...
zeros(n) m2*Beta*H1*W1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,4*n) -Landa2*eye(n) zeros(n) zeros(n,(dM-dm)*n);...
zeros(n,2*n) m2*G11bar*Z21 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,5*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,(dM-dm)*n);...
zeros((dM-dm)*n,3*n) blkdiag(W2,W2) zeros((dM-dm)*n,(dm-1)*n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,3*n) zeros((dM-dm)*n,l) zeros((dM-dm)*n,6*n) blkdiag(-W1,-W1)]<=0];

%LMI#1      j=2;s=1;
C=C+[[-Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,2*n) zeros(n,(dM-1)*n) m1*(A20*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')+B20*Y2*C2)' (Y2*C2)'*R2 (Z12*C2)' Vp2*X12*Vp2'+Vpp2*X22*Vpp2' Vp2*X12*Vp2'+Vpp2*X22*Vpp2' (C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))'*R1 (m2*E12bar*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))' (m2*G12bar*Y2*C2)' (m2*Alpha*F2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))' zeros(n,3*n) zeros(n,(dM-dm)*n);...
zeros(n) -W1 zeros(n) zeros(n,(dM-1)*n) m1*(Ad20*W1)' zeros(n,m) zeros(n,3*n)  zeros(n,l) zeros(n,3*n) (m2*F12bar*W1)' (m2*Beta*H2*W1)' zeros(n) zeros(n,(dM-dm)*n);...
zeros(n,2*n) -Sz2 zeros(n,(dM-1)*n) m1*(B20*Z22)' Z22'*R2  Z32' zeros(n,2*n) zeros(n,l) zeros(n,5*n) (m2*G12bar*Z22)' zeros(n,(dM-dm)*n);...
zeros(n*(dM-dm),3*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n*(dm-1)) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),3*n) zeros(n*(dM-dm),l) zeros(n*(dM-dm),6*n) blkdiag(W2,W2);...
zeros(n*(dm-1),3*n) zeros(n*(dm-1),n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),3*n) zeros(n*(dm-1),l) zeros(n*(dm-1),6*n) zeros(n*(dm-1),(dM-dm)*n);...
m1*(A20*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')+B20*Y2*C2) m1*(Ad20*W1) m1*(B20*Z22) zeros(n,(dM-1)*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
R2*(Y2*C2) zeros(m,n) R2*Z22 zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,3*n) zeros(m,l) zeros(m,6*n) zeros(m,(dM-dm)*n);...
Z12*C2 zeros(n) Z32 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -Sz1 zeros(n,2*n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
Vp2*X12*Vp2'+Vpp2*X22*Vpp2' zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W1 zeros(n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
Vp2*X12*Vp2'+Vpp2*X22*Vpp2' zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) -((dM-1)^(-1))*W2 zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
R1*(C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')) zeros(l,2*n) zeros(l,(dM-1)*n) zeros(l,n) zeros(l,m) zeros(l,3*n) -gama*eye(l) zeros(l,6*n) zeros(l,(dM-dm)*n);...
m2*E12bar*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,5*n) zeros(n,(dM-dm)*n);...
m2*G12bar*Y2*C2 zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,4*n) zeros(n,(dM-dm)*n);...
m2*Alpha*F2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,2*n) -Landa1*eye(n) zeros(n,3*n) zeros(n,(dM-dm)*n);...
zeros(n) m2*F12bar*W1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,3*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,2*n) zeros(n,(dM-dm)*n);...
zeros(n) m2*Beta*H2*W1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,4*n) -Landa1*eye(n) zeros(n) zeros(n,(dM-dm)*n);...
zeros(n,2*n) m2*G12bar*Z22 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,5*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,(dM-dm)*n);...
zeros((dM-dm)*n,3*n) blkdiag(W2,W2) zeros((dM-dm)*n,(dm-1)*n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,3*n) zeros((dM-dm)*n,l) zeros((dM-dm)*n,6*n) blkdiag(-W1,-W1)]<=0];

%LMI#1      j=2;s=2;
C=C+[[-Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,2*n) zeros(n,(dM-1)*n) m1*(A20*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')+B20*Y2*C2)' (Y2*C2)'*R2 (Z12*C2)' Vp2*X12*Vp2'+Vpp2*X22*Vpp2' Vp2*X12*Vp2'+Vpp2*X22*Vpp2' (C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))'*R1 (m2*E12bar*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))' (m2*G12bar*Y2*C2)' (m2*Alpha*F2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))' zeros(n,3*n) zeros(n,(dM-dm)*n);...
zeros(n) -W1 zeros(n) zeros(n,(dM-1)*n) m1*(Ad20*W1)' zeros(n,m) zeros(n,3*n)  zeros(n,l) zeros(n,3*n) (m2*F12bar*W1)' (m2*Beta*H2*W1)' zeros(n) zeros(n,(dM-dm)*n);...
zeros(n,2*n) -Sz2 zeros(n,(dM-1)*n) m1*(B20*Z22)' Z22'*R2  Z32' zeros(n,2*n) zeros(n,l) zeros(n,5*n) (m2*G12bar*Z22)' zeros(n,(dM-dm)*n);...
zeros(n*(dM-dm),3*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n*(dm-1)) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),3*n) zeros(n*(dM-dm),l) zeros(n*(dM-dm),6*n) blkdiag(W2,W2);...
zeros(n*(dm-1),3*n) zeros(n*(dm-1),n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),3*n) zeros(n*(dm-1),l) zeros(n*(dm-1),6*n) zeros(n*(dm-1),(dM-dm)*n);...
m1*(A20*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')+B20*Y2*C2) m1*(Ad20*W1) m1*(B20*Z22) zeros(n,(dM-1)*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
R2*(Y2*C2) zeros(m,n) R2*Z22 zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,3*n) zeros(m,l) zeros(m,6*n) zeros(m,(dM-dm)*n);...
Z12*C2 zeros(n) Z32 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -Sz2 zeros(n,2*n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
Vp2*X12*Vp2'+Vpp2*X22*Vpp2' zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W1 zeros(n) zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
Vp2*X12*Vp2'+Vpp2*X22*Vpp2' zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) -((dM-1)^(-1))*W2 zeros(n,l) zeros(n,6*n) zeros(n,(dM-dm)*n);...
R1*(C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')) zeros(l,2*n) zeros(l,(dM-1)*n) zeros(l,n) zeros(l,m) zeros(l,3*n) -gama*eye(l) zeros(l,6*n) zeros(l,(dM-dm)*n);...
m2*E12bar*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,5*n) zeros(n,(dM-dm)*n);...
m2*G12bar*Y2*C2 zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,4*n) zeros(n,(dM-dm)*n);...
m2*Alpha*F2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n,2*n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,2*n) -Landa2*eye(n) zeros(n,3*n) zeros(n,(dM-dm)*n);...
zeros(n) m2*F12bar*W1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,3*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,2*n) zeros(n,(dM-dm)*n);...
zeros(n) m2*Beta*H2*W1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,4*n) -Landa2*eye(n) zeros(n) zeros(n,(dM-dm)*n);...
zeros(n,2*n) m2*G12bar*Z22 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,3*n) zeros(n,l) zeros(n,5*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,(dM-dm)*n);...
zeros((dM-dm)*n,3*n) blkdiag(W2,W2) zeros((dM-dm)*n,(dm-1)*n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,3*n) zeros((dM-dm)*n,l) zeros((dM-dm)*n,6*n) blkdiag(-W1,-W1)]<=0];

%LMI#2  j=1,j=2;
C=C+[(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')-((zita1/eps1)*eye(n))>=0,(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')-((zita1/eps1)*eye(n))>=0,Sz1-((zita2/eps2)*eye(n))>=0,Sz2-((zita2/eps2)*eye(n))>=0];                     %New LMI
C=C+[W1-(((zita1*dM)/eps3)*eye(n))>=0,W2-(((zita1*dM*(dM-1))/(2-2*eps1-2*eps2-2*eps3))*eye(n))>=0];                                                                                                %New LMI

%LMI#3  s=1,s=2;
C=C+[Vp1*X11*Vp1'+Vpp1*X21*Vpp1'-Landa1*eye(n)>=0, Vp2*X12*Vp2'+Vpp2*X22*Vpp2'-Landa2*eye(n)>=0];

if Cu==1
    
    %input constraint  j=1;
    C=C+[[G1 Y1*C1 Z21;...
        (Y1*C1)' Vp1*X11*Vp1'+Vpp1*X21*Vpp1' zeros(n);...
        Z21' zeros(n) Sz1]>=0];
    
    %input constraint  j=2;
    C=C+[[G1 Y2*C2 Z22;...
        (Y2*C2)' Vp2*X12*Vp2'+Vpp2*X22*Vpp2' zeros(n);...
        Z22' zeros(n) Sz2]>=0];
    
    C=C+[[G1-blkdiag(u1max^2)]<=0];
    
end

if Cy==1
    
    %output constraint  j=1;
    C=C+[[G2 C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1');...
        (Vp1*X11*Vp1'+Vpp1*X21*Vpp1')*C1' (Vp1*X11*Vp1'+Vpp1*X21*Vpp1')]>=0];
    
    %output constraint  j=2;
    C=C+[[G2 C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2');...
        (Vp2*X12*Vp2'+Vpp2*X22*Vpp2')'*C2' (Vp2*X12*Vp2'+Vpp2*X22*Vpp2')]>=0];
    
    C=C+[[G2-blkdiag(y1max^2)]<=0];
    
end

o=gama;
op=sdpsettings('solver','lmilab,*');
diag=optimize(C,gama,op)

Y1v=value(Y1);Y2v=value(Y2);
%Sx1v=value(Sx1);Sx2v=value(Sx2);
X11v=value(X11);X12v=value(X12);
X21v=value(X21);X22v=value(X22);
Sz1v=value(Sz1);Sz2v=value(Sz2);
W1v=value(W1);W2v=value(W2);
Landa1v=value(Landa1);Landa2v=value(Landa2);
gamav=value(gama);
Z11v=value(Z11);Z12v=value(Z12);
Z21v=value(Z21);Z22v=value(Z22);
Z31v=value(Z31);Z32v=value(Z32);

Sx1v=Vp1*X11v*Vp1'+Vpp1*X21v*Vpp1';
Sx2v=Vp2*X12v*Vp2'+Vpp2*X22v*Vpp2';

% X11=V1'*Sx1v*V1;X11=X11(1,1);
% X12=V2'*Sx2v*V2;X12=X12(1,1);

Ac1=Z31v*inv(Sz1v);Ac2=Z32v*inv(Sz2v);
Cc1=Z21v*inv(Sz1v);Cc2=Z22v*inv(Sz2v);
Dc1=Y1v*U1*C10*inv(X11v)*inv(C10)*U1';
Dc2=Y2v*U2*C20*inv(X12v)*inv(C20)*U2';
Bc1=Z11v*U1*C10*inv(X11v)*inv(C10)*U1';
Bc2=Z12v*U2*C20*inv(X12v)*inv(C20)*U2';

for i=T0:Tf
    
    delta=el*sin(i);
    z=mod(i,2);
    if (z>0)
        zDOF(:,i)=Ac1*zDOF(:,i-1)+Bc1*C1*xDOF(:,i-1);
        xDOF(:,i)=(A10+delta*E11)*xDOF(:,i-1)+(Ad10+delta*F11)*xDOF(:,i-1-d(i))+(B10+delta*G11)*Dc1*C1*xDOF(:,i-1)+(B10+delta*G11)*Cc1*zDOF(:,i-1)+[0.01*sin(xDOF(1,i-1));0]+[0.002*sin(xDOF(1,i-1-d(i)));0];
        uDOF(:,i-1)=Cc1*zDOF(:,i-1)+Dc1*C1*xDOF(:,i-1);
        yDOF(:,i-1)=C1*xDOF(:,i-1);
        nDOF(i) =norm(xDOF(:,i));
        sDOF(i)=1;
        VDOF(i-1)=xDOF(:,i-1)'*gamav*inv(Sx1v)*xDOF(:,i-1);
        VDOF(i-1)=VDOF(i-1)+zDOF(:,i-1)'*gamav*inv(Sz1v)*zDOF(:,i-1);
        for k=-1:-1:-dM
            VDOF(i-1)=VDOF(i-1)+xDOF(:,i-1+k)'*gamav*inv(W1v)*xDOF(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            VDOF(i-1)=VDOF(i-1)+(dM+k)*xDOF(:,i-1+k)'*gamav*inv(W2v)*xDOF(:,i-1+k);
        end
        VDOF(i-1);
    else
        zDOF(:,i)=Ac2*zDOF(:,i-1)+Bc2*C2*xDOF(:,i-1);
        xDOF(:,i)=(A20+delta*E12)*xDOF(:,i-1)+(Ad20+delta*F12)*xDOF(:,i-1-d(i))+(B20+delta*G12)*Dc2*C2*xDOF(:,i-1)+(B20+delta*G12)*Cc2*zDOF(:,i-1)+[0.01*sin(xDOF(1,i-1));0]+[0.002*sin(xDOF(1,i-1-d(i)));0];
        uDOF(:,i-1)=Cc2*zDOF(:,i-1)+Dc2*C2*xDOF(:,i-1);
        yDOF(:,i-1)=C2*xDOF(:,i-1);
        nDOF(i) =norm(xDOF(:,i));
        sDOF(i)=2;
        VDOF(i-1)=xDOF(:,i-1)'*gamav*inv(Sx2v)*xDOF(:,i-1);
        VDOF(i-1)=VDOF(i-1)+zDOF(:,i-1)'*gamav*inv(Sz2v)*zDOF(:,i-1);
        for k=-1:-1:-dM
            VDOF(i-1)=VDOF(i-1)+xDOF(:,i-1+k)'*gamav*inv(W1v)*xDOF(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            VDOF(i-1)=VDOF(i-1)+(dM+k)*xDOF(:,i-1+k)'*gamav*inv(W2v)*xDOF(:,i-1+k);
        end
        VDOF(i-1);
    end
    JDOF=JDOF+yDOF(:,i-1)'*R1*yDOF(:,i-1)+uDOF(:,i-1)'*R2*uDOF(:,i-1);
end

save('Constrained_RMPC_DOF.mat')
CostFunction_dynamicy_InequalQ=JDOF;

uDOF1max=max(uDOF(1,T0-1:Tf-1));
uDOF1min=min(uDOF(1,T0-1:Tf-1));
uDOF1Max=max(abs(uDOF1min),abs(uDOF1max));

yDOF1max=max(yDOF(1,T0:Tf-1));
yDOF1min=min(yDOF(1,T0:Tf-1));
yDOF1Max=max(abs(yDOF1min),abs(yDOF1max));

VDOFMax=max(VDOF(T0-1:Tf-1));

List={};
Result_table = table(dm,dM,CostFunction_dynamicy_InequalQ,uDOF1Max,yDOF1Max,gamav,VDOFMax,'RowNames',List)

t=0:Tf-T0;
figure;stairs(t,xDOF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('States');legend('x_1','x_2')
%figure;stairs(t,sDOF(T0:Tf),'LineWidth',2);grid on;xlabel('k (sample)');ylabel('Switching signal');ylim([0 3])
figure;stairs(t,uDOF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Control Signal (u)');
figure;stairs(t,yDOF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Output (y)');
%figure;plot(1:50,d,'LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('d(k)');
figure;stairs(t,VDOF(T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('V');
%figure;plot(t,nDOF(T0:Tf)','*');grid on;xlabel('k (sample)');ylabel('Norm of state variables');